// Dynamixel SDK platform dependent source
#include "dxl_hal.h"
#include "Serial.hh"
#include <sys/time.h>
#include <unistd.h>
#include <time.h>

static int dxl_fd = -1;
static float dxl_baudrate;
static timeval dxl_timeout_tv;

int dxl_hal_open( int fileDescriptor, float baudrate )
{
	// Opening device
	// devIndex: Device index
	// baudrate: Real baudrate (ex> 115200, 57600, 38400...)
	// Return: 0(Failed), 1(Succeed)

	dxl_fd = fileDescriptor;
	dxl_baudrate = baudrate;
	return dxl_fd != -1;
}

void dxl_hal_close()
{
	// Closing device
	dxl_fd = -1;
}

void dxl_hal_clear(void)
{
	// Clear communication buffer

}

int dxl_hal_tx( unsigned char *pPacket, int numPacket )
{
	// Transmiting date
	// *pPacket: data array pointer
	// numPacket: number of data array
	// Return: number of data transmitted. -1 is error.
	int rc = write(dxl_fd, pPacket, numPacket);

	timespec ts;
	ts.tv_sec=0;
	ts.tv_nsec=1000000; // 1ms
	nanosleep(&ts,0);

	return rc;
}

int dxl_hal_rx( unsigned char *pPacket, int numPacket )
{
	// Recieving date
	// *pPacket: data array pointer
	// numPacket: number of data array
	// Return: number of data recieved. -1 is error.
	int rc = read(dxl_fd, pPacket, numPacket);
	return rc;
}

void dxl_hal_set_timeout( int NumRcvByte )
{
	// Start stop watch
	// NumRcvByte: number of recieving data(to calculate maximum waiting time)
	timeval tv;
	gettimeofday(&tv, NULL);
	long usecs_to_timeout = (1+NumRcvByte)*10.0/dxl_baudrate + tv.tv_usec;

	tv.tv_usec += usecs_to_timeout;

	while(tv.tv_usec > 1000000)
	{
		tv.tv_sec ++;
		tv.tv_usec -= 1000000;
	}
	dxl_timeout_tv = tv;
}

int dxl_hal_timeout(void)
{
	// Check timeout
	// Return: 0 is false, 1 is true(timeout occurred)

	timeval tv;
	gettimeofday(&tv, NULL);

	if( (tv.tv_sec > dxl_timeout_tv.tv_sec) || ((tv.tv_sec == dxl_timeout_tv.tv_sec) && (tv.tv_usec > dxl_timeout_tv.tv_usec) ))
		return 1;
	else
		return 0;
}
